////////////////////////////////////////////////////////////////////////////
//
//
//    Project     : VideoNet version 1.1.
//    Description : Peer to Peer Video Conferencing over the LAN.
//	  Author      :	Nagareshwar Y Talekar ( nsry2002@yahoo.co.in)
//    Date        : 15-6-2004.
//
//
//    File description : 
//    Name    : convert.cpp
//    Details : Conversion routine from RGB24 to YUV420 & YUV420 to RGB24.
//
/////////////////////////////////////////////////////////////////////////////


#include "convert.h"

// Conversion from RGB to YUV420
int RGB2YUV_YR[256], RGB2YUV_YG[256], RGB2YUV_YB[256];
int RGB2YUV_UR[256], RGB2YUV_UG[256], RGB2YUV_UBVR[256];
int RGB2YUV_VG[256], RGB2YUV_VB[256];

// Conversion from YUV420 to RGB24
static long int crv_tab[256];
static long int cbu_tab[256];
static long int cgu_tab[256];
static long int cgv_tab[256];
static long int tab_76309[256];
static unsigned char clp[1024];			//for clip in CCIR601


//
// Table used for RGB to YUV420 conversion
//
void InitLookupTable()
{
	int i;

	for (i = 0; i < 256; i++) RGB2YUV_YR[i] = (float)65.481 * (i<<8);
	for (i = 0; i < 256; i++) RGB2YUV_YG[i] = (float)128.553 * (i<<8);
	for (i = 0; i < 256; i++) RGB2YUV_YB[i] = (float)24.966 * (i<<8);
	for (i = 0; i < 256; i++) RGB2YUV_UR[i] = (float)37.797 * (i<<8);
	for (i = 0; i < 256; i++) RGB2YUV_UG[i] = (float)74.203 * (i<<8);
	for (i = 0; i < 256; i++) RGB2YUV_VG[i] = (float)93.786 * (i<<8);
	for (i = 0; i < 256; i++) RGB2YUV_VB[i] = (float)18.214 * (i<<8);
	for (i = 0; i < 256; i++) RGB2YUV_UBVR[i] = (float)112 * (i<<8);
}

unsigned char uu[176*144];
unsigned char vv[176*144];

//
//  Convert from  RGB24 to YUV420
//
int ConvertRGB2YUV(int w,int h,unsigned char *bmp,unsigned char *py, unsigned char *pu, unsigned char *pv)
{
	int i,j;
	unsigned char r,g,b;

	//Get YUV values for rgb values...
	for(i=0;i<h;i++)
	{
		for(j=0;j<w;j++)
		{
			r = bmp[3*i*w+3*j];
			g = bmp[3*i*w+3*j+1];
			b = bmp[3*i*w+3*j+2];

			py[i*w+j] = ( RGB2YUV_YR[r]  +RGB2YUV_YG[g]+RGB2YUV_YB[b]+1048576)>>16;
			uu[i*w+j] = (-RGB2YUV_UR[r]  -RGB2YUV_UG[g]+RGB2YUV_UBVR[b]+8388608)>>16;
			vv[i*w+j] = ( RGB2YUV_UBVR[r]-RGB2YUV_VG[g]-RGB2YUV_VB[b]+8388608)>>16;
		}
	}

	// Now sample the U & V to obtain YUV 4:2:0 format

	// Sampling mechanism...
/*	  @  ->  Y
	  #  ->  U or V
	  
	  @   @   @   @
		#       #
	  @   @   @   @
	
	  @   @   @   @
		#       #
	  @   @   @   @

*/
	// Do sampling....
	for(i=0;i<h/2;i++)
	{
		for(j=0;j<w/2;j++)
		{
			pu[i*w/2+j] = (uu[2*i*w+2*j] + uu[2*i*w+2*j+1] + uu[2*i*w+2*j+w] + uu[2*i*w+2*j+w+1]) >> 2;
			pv[i*w/2+j] = (vv[2*i*w+2*j] + vv[2*i*w+2*j+1] + vv[2*i*w+2*j+w] + vv[2*i*w+2*j+w+1]) >> 2;
		}
	}

	return 1;
}

//
//Initialize conversion table for YUV420 to RGB
//
void InitConvertTable()
{
   long int crv,cbu,cgu,cgv;
   int i,ind;   
     
   crv = 104597; cbu = 132201;  /* fra matrise i global.h */
   cgu = 25675;  cgv = 53279;
  
   for (i = 0; i < 256; i++) {
      crv_tab[i] = (i-128) * crv;
      cbu_tab[i] = (i-128) * cbu;
      cgu_tab[i] = (i-128) * cgu;
      cgv_tab[i] = (i-128) * cgv;
      tab_76309[i] = 76309*(i-16);
   }
	 
   for (i=0; i<384; i++)
	  clp[i] =0;
   ind=384;
   for (i=0;i<256; i++)
	   clp[ind++]=i;
   ind=640;
   for (i=0;i<384;i++)
	   clp[ind++]=255;
}


//
//  Convert from YUV420 to RGB24
//
void ConvertYUV2RGB(unsigned char *src0,unsigned char *src1,unsigned char *src2,unsigned char *dst_ori,int width,int height)
{
	int y1,y2,u,v; 
	unsigned char *py1,*py2;
	int i,j, c1, c2, c3, c4;
	unsigned char *d1, *d2;

	py1=src0;
	py2=py1+width;
	d1=dst_ori;
	d2=d1+3*width;
 	for (j = 0; j < height; j += 2) 
	{ 
		for (i = 0; i < width; i += 2) 
		{

			u = *src1++;
			v = *src2++;

			c1 = crv_tab[v];
			c2 = cgu_tab[u];
			c3 = cgv_tab[v];
			c4 = cbu_tab[u];

			//up-left
            y1 = tab_76309[*py1++];	
			*d1++ = clp[384+((y1 + c1)>>16)];  
			*d1++ = clp[384+((y1 - c2 - c3)>>16)];
            *d1++ = clp[384+((y1 + c4)>>16)];

			//down-left
			y2 = tab_76309[*py2++];
			*d2++ = clp[384+((y2 + c1)>>16)];  
			*d2++ = clp[384+((y2 - c2 - c3)>>16)];
            *d2++ = clp[384+((y2 + c4)>>16)];

			//up-right
			y1 = tab_76309[*py1++];
			*d1++ = clp[384+((y1 + c1)>>16)];  
			*d1++ = clp[384+((y1 - c2 - c3)>>16)];
			*d1++ = clp[384+((y1 + c4)>>16)];

			//down-right
			y2 = tab_76309[*py2++];
			*d2++ = clp[384+((y2 + c1)>>16)];  
			*d2++ = clp[384+((y2 - c2 - c3)>>16)];
            *d2++ = clp[384+((y2 + c4)>>16)];
		}
		d1 += 3*width;
		d2 += 3*width;
		py1+=   width;
		py2+=   width;
	}       


}

// ================= MEMBER FUNCTIONS =======================
//
int FindStartCode (unsigned char *Buf, int zeros_in_startcode)
{
	int info;
	int i;

	info = 1;
	for (i = 0; i < zeros_in_startcode; i++)
		if(Buf[i] != 0)
			info = 0;

	if(Buf[i] != 1)
		info = 0;
	return info;
}

int getNextNal(unsigned char *data, int size, unsigned char* Buf)
{
	int pos = 0;
	int StartCodeFound = 0;
	int info2 = 0;
	int info3 = 0;

	while(pos < size)
	{
		Buf[pos] = data[pos];
		if ( Buf[pos++] != 0 )
		{
			break;
		}
	}

	while (!StartCodeFound)
	{
		if (pos > size)
		{
			return pos-1;
		}

		Buf[pos] = data[pos];
		pos++;

		info3 = FindStartCode(&Buf[pos-4], 3);
		if(info3 != 1)
		{
			info2 = FindStartCode(&Buf[pos-3], 2);
		}
		StartCodeFound = (info2 == 1 || info3 == 1);
	}

	return pos - 4;
}
